﻿using System;

using Microsoft.Xna.Framework;

namespace OakEngine.AI.Tools
{
   public static class OAK_AITools
    {
        public static float TurnToAim(Vector2 currentPosition, Vector2 targetPosition, float currentAngle, float turnspeed = 0.025f)
        {
            Vector2 turretMtower = targetPosition - currentPosition;

            float angle = (float)Math.Atan2((double)turretMtower.Y, (double)turretMtower.X);

            float whereToTurn = WrapAngle(angle - currentAngle);

            whereToTurn = MathHelper.Clamp(whereToTurn, -turnspeed, turnspeed);


            return WrapAngle(currentAngle + whereToTurn);
        }

        public static float WrapAngle(float radians)
        {
            while (radians < -MathHelper.Pi)
            {
                radians += MathHelper.TwoPi;
            }
            while (radians > MathHelper.Pi)
            {
                radians -= MathHelper.TwoPi;
            }

            return radians;
        }

        //	Rotate a vector
        /*
            X, Y	=	[X]
                        [Y]
                        [1]

            [	cos&	sin&	0	]		[X]		[X1]
            [  -sin&	cos&	0	]	*	[Y]	=	[Y1]
            [	0		0		1	]		[1]		[1]
        */
        public static Vector2 Vector2DRotate(Vector2 vec, float fRadians)
        {
            //	Convert into the Cartesian coordinate system
            vec.Y *= -1.0f;
            double x = (Math.Cos(fRadians) * vec.X) + (Math.Sin(fRadians) * vec.Y);
            double y = (-Math.Sin(fRadians) * vec.X) + (Math.Cos(fRadians) * vec.Y);
            
            Vector2 vRotated;
            vRotated.X = (float)x;
            vRotated.Y = (float)y;

            //	Convert back to our windows coordinate system
            vRotated.Y *= -1.0f;

            //	Return our rotated vector
            return vRotated;
        }

        public static float SquareMagnitude(Vector2 vec)
        {
            return (vec.X*vec.X + vec.Y * vec.Y);
        }


    }
}
